//-----------------------------------------------------------------------------
// Includes
//-----------------------------------------------------------------------------
#include <math.h>
#include <Math/Matrix.hpp>
#include <Constants.hpp>

	//-------------------------------------------------------------------------	
	Matrix::Matrix4 Matrix::identityMatrix4 = 
	{
		1.f, 0.f, 0.f, 0.f,	// C0
		0.f, 1.f, 0.f, 0.f,	// C1
		0.f, 0.f, 1.f, 0.f,	// C2
		0.f, 0.f, 0.f, 1.f	// C3
	};
	//-------------------------------------------------------------------------
	const Matrix::Matrix4 Matrix::Identity = 
	{
		1.f, 0.f, 0.f, 0.f,	// C0
		0.f, 1.f, 0.f, 0.f,	// C1
		0.f, 0.f, 1.f, 0.f,	// C2
		0.f, 0.f, 0.f, 1.f	// C3
	};
	//-------------------------------------------------------------------------
	void Matrix::Copy4(float *a, const float *b)
	{
		Vector::Copy4(&a[0], &b[0]);
		Vector::Copy4(&a[4], &b[4]);
		Vector::Copy4(&a[8], &b[8]);
		Vector::Copy4(&a[12], &b[12]);
	}
	//-------------------------------------------------------------------------
	void Matrix::Zero4(float *a)
	{
		Vector::Zero4(&a[0]);
		Vector::Zero4(&a[4]);
		Vector::Zero4(&a[8]);
		Vector::Zero4(&a[12]);
	}
	//-------------------------------------------------------------------------
	void Matrix::Identity4(float *a)
	{
		Copy4(a, Matrix::identityMatrix4);
	}
	//-------------------------------------------------------------------------
	void Matrix::Identity3(float *a)
	{
		Vector::Copy4(&a[0], &Matrix::identityMatrix4[0]);
		Vector::Copy4(&a[4], &Matrix::identityMatrix4[4]);
		Vector::Copy4(&a[8], &Matrix::identityMatrix4[8]);
		Vector::Zero4(&a[12]);
	}
	//-------------------------------------------------------------------------
	void Matrix::Mul4(float *a, const float *b, const float *c)
	{
		int i, j;

		for (i = 0; i < 4; i++) {
			for (j = 0; j < 4; j++) {
				a[i*4+j] = 
					b[i*4+0] * c[0*4+j] +
					b[i*4+1] * c[1*4+j] +
					b[i*4+2] * c[2*4+j] +
					b[i*4+3] * c[3*4+j];
			}
		}	
	}
	//-------------------------------------------------------------------------
	void Matrix::ConstMul4(float *a, float k, const float *b)
	{
		Vector::ConstMul4(&a[0], k, &b[0]);
		Vector::ConstMul4(&a[4], k, &b[4]);
		Vector::ConstMul4(&a[8], k, &b[8]);
		Vector::ConstMul4(&a[12], k, &b[12]);
	}
	//-------------------------------------------------------------------------
	void Matrix::Compose4(float *a, const float *b)
	{
		float t[16];
		Mul4(t, a, b);
		Copy4(a, t);
	}
	//-------------------------------------------------------------------------
	void Matrix::MulVec4(float *v, const float *a, const float *u)
	{
		int i;
		for (i=0; i < 4; i++) {
			v[i]= u[0]*a[0*4+i] 
				+ u[1]*a[1*4+i]
				+ u[2]*a[2*4+i]
				+ u[3]*a[3*4+i];
		}
	}
	//-------------------------------------------------------------------------
	void Matrix::ComposePoint4(const float *a, float *u)
	{
		float tmp[4];
		int i;
		for(i=0; i < 4; i++) {
			tmp[i]= u[0]*a[0*4+i] 
				+ u[1]*a[1*4+i]
				+ u[2]*a[2*4+i]
				+ u[3]*a[3*4+i];
		}
		Copy4(u, tmp);
	}
	//-------------------------------------------------------------------------
	void Matrix::ComposeVector4(const float *a, float *u)
	{
		float tmp[4];
		int i;
		for(i=0; i < 3; i++) 
			tmp[i]= u[0]*a[0*4+i] 
				+ u[1]*a[1*4+i]
				+ u[2]*a[2*4+i];
		tmp[3]= 0.f;
		Copy4(u, tmp);
	}
	//-------------------------------------------------------------------------
	void Matrix::InitVec4Tranpose4(float *a, const float *v)
	{
		int i;
		for(i= 0; i < 4; i++)
		{
			M44(a, i, 0)= v[i] * v[0];
			M44(a, i, 1)= v[i] * v[1];
			M44(a, i, 2)= v[i] * v[2];
			M44(a, i, 3)= v[i] * v[3];
		}
	}
	//-------------------------------------------------------------------------
	void Matrix::InitVec3Tranpose4(float *a, const float *v)
	{
		float t[4];
		Vector::Init4(t, v[0], v[1], v[2], 0.f);
		InitVec4Tranpose4(a, t);
	}
	//-------------------------------------------------------------------------
	void Matrix::Sub4(float *a, const float *b, const float *c)
	{
		Vector::Sub4(&a[0], &b[0], &c[0]);
		Vector::Sub4(&a[4], &b[4], &c[4]);
		Vector::Sub4(&a[8], &b[8], &c[8]);
		Vector::Sub4(&a[12], &b[12], &c[12]);
	}
	//-------------------------------------------------------------------------
	void Matrix::Add4(float *a, const float *b, const float *c)
	{
		Vector::Add4(&a[0], &b[0], &c[0]);
		Vector::Add4(&a[4], &b[4], &c[4]);
		Vector::Add4(&a[8], &b[8], &c[8]);
		Vector::Add4(&a[12], &b[12], &c[12]);
	}
	//-------------------------------------------------------------------------
	int Matrix::Inverse4(float *inverse, const float *src)
	{
		int i, j, k, swap;
		float t;
		float temp[4][4];
		for (i=0; i<4; i++) {
				for (j=0; j<4; j++) {
					temp[i][j] = src[i*4+j];
				}
		}
		Identity4(inverse);
		for (i = 0; i < 4; i++) {
			// Look for largest element in column 
			swap = i;
			for (j = i + 1; j < 4; j++)	{
				if (fabs(temp[j][i]) > fabs(temp[i][i]))
					swap = j;
			}
			if (swap != i) {
				// Swap rows.
				for (k = 0; k < 4; k++)	{
					t = temp[i][k];
					temp[i][k] = temp[swap][k];
					temp[swap][k] = t;
					t = inverse[i*4+k];
					inverse[i*4+k] = inverse[swap*4+k];
					inverse[swap*4+k] = t;
				}
			}
			if (fabs(temp[i][i]) < 1e-10) {
				// No non-zero pivot.  The matrix is singular, which 
				// shouldn't happen.  This means the user gave us a bad matrix.
				return -1;
			}
			t = temp[i][i];
			for (k = 0; k < 4; k++)	{
				temp[i][k] /= t;
				inverse[i*4+k] /= t;
			}
			for (j = 0; j < 4; j++)	{
				if (j != i)	{
					t = temp[j][i];
					for (k = 0; k < 4; k++)	{
						temp[j][k] -= temp[i][k]*t;
						inverse[j*4+k] -= inverse[i*4+k]*t;
					}
				}
			}
		}
		return 0;
	}
	//-------------------------------------------------------------------------
	void Matrix::Print4(const float *a)
	{
		printf("% -.8f  % -.8f  % -.8f  % -.8f\n", 
			M44(a, 0, 0), M44(a, 0, 1), M44(a, 0, 2), M44(a, 0, 3));
		printf("% -.8f  % -.8f  % -.8f  % -.8f\n", 
			M44(a, 1, 0), M44(a, 1, 1), M44(a, 1, 2), M44(a, 1, 3));
		printf("% -.8f  % -.8f  % -.8f  % -.8f\n", 
			M44(a, 2, 0), M44(a, 2, 1), M44(a, 2, 2), M44(a, 2, 3));
		printf("% -.8f  % -.8f  % -.8f  % -.8f\n", 
			M44(a, 3, 0), M44(a, 3, 1), M44(a, 3, 2), M44(a, 3, 3));
		printf("\n");
	}	
	//-------------------------------------------------------------------------
	void Matrix::Scale4(float *a, float x, float y, float z)
	{
		Identity4(a);
		M44(a, 0, 0)= x;
		M44(a, 1, 1)= y;
		M44(a, 2, 2)= z;
	}
	//-------------------------------------------------------------------------
	void Matrix::Translate4(float *a, float x, float y, float z)
	{
		Identity4(a);
		M44(a, 0, 3)= x;
		M44(a, 1, 3)= y;
		M44(a, 2, 3)= z;
	}
	//-------------------------------------------------------------------------
	void Matrix::Rotate4(float *a, float angle, float x, float y, float z)
	{
		float S[16];
		float C[16];
		float R[16];
		Vector::Vector3 u;
		float sinr, cosr;
		// R= u.u^T
		Vector::Init3(u, x, y, z);
		if(Vector::SquareLength3(u) < .00001f) {
			Identity4(a);
			return;
		}	    
		Vector::Norm3(u, u);
		Matrix::InitVec3Tranpose4(R, u);	    
		// S
		Zero4(S);
		//sinr = sinf(angle/180.0f * M_PI);		
		sinr = sinf(angle/180.0f * PI);
		Vector::ConstMul3(u, sinr, u);
		M44(S, 0, 1)= -u[2];
		M44(S, 0, 2)=  u[1];
		M44(S, 1, 0)=  u[2];
		M44(S, 1, 2)= -u[0];
		M44(S, 2, 0)= -u[1];
		M44(S, 2, 1)=  u[0];
	    
		// C
		Identity4(C);
		Sub4(C, C, R);
		cosr= cosf(angle / 180.0f * PI);
		ConstMul4(C, cosr, C);

		// a= R + C + S
		Add4(a, R, C);
		Add4(a, a, S);
		M44(a, 3, 3)= 1.f;
	}
	//-------------------------------------------------------------------------
	void Matrix::LookAt4(float *a, 
		const float *eye, const float *center, const float *up)
	{
		float forward[4];
		float side[4];
		float upn[4];
	    
		Vector::Sub3(forward, center, eye);
		Vector::Norm3(forward, forward);
	    
		Vector::Cross3(side, forward, up);
		Vector::Norm3(side, side);
	    
		Vector::Cross3(upn, side, forward);
	    
		Identity4(a);
		M44(a, 0, 0)= side[0];
		M44(a, 0, 1)= side[1];
		M44(a, 0, 2)= side[2];
	    
		M44(a, 1, 0)= upn[0];
		M44(a, 1, 1)= upn[1];
		M44(a, 1, 2)= upn[2];
	    
		M44(a, 2, 0)= -forward[0];
		M44(a, 2, 1)= -forward[1];
		M44(a, 2, 2)= -forward[2];
	    
		M44(a, 0, 3)= -Vector::Dot3(eye, side);
		M44(a, 1, 3)= -Vector::Dot3(eye, upn);
		M44(a, 2, 3)= Vector::Dot3(eye, forward);
	}
	//-------------------------------------------------------------------------
	void Matrix::Frustum4(float *a, 
		float left, float right, 
		float bottom, float top, 
		float near, float far)
	{
		Zero4(a);
		M44(a, 0, 0)= 2.f*near / (right - left);
		M44(a, 0, 2)= (right + left) / (right - left);
		M44(a, 1, 1)= 2.f*near / (top - bottom);
		M44(a, 1, 2)= (top + bottom) / (top - bottom);
		M44(a, 2, 2)= -(far + near) / (far - near);
		M44(a, 2, 3)= -2.f*far*near / (far - near);
		M44(a, 3, 2)= -1.f;
	}
	//-------------------------------------------------------------------------
	void Matrix::Perspective4(float *a, float fovy, float aspect, 
		float near, float far)
	{
		float xmin, xmax, ymin, ymax;
		ymax = near * tanf(fovy / 2.f * PI / 180.f);
		ymin = -ymax;
		xmin = ymin * aspect;
		xmax = ymax * aspect;
		Frustum4(a, xmin, xmax, ymin, ymax, near, far);
	}
	//-------------------------------------------------------------------------
	void Matrix::LinearPerspective4(float *a, float fovy, float aspect, float near, float far) {
		Perspective4(a,fovy,aspect,near,far);
		M44(a, 2, 2) *= 1.f/ far;
		M44(a, 2, 3) *= 1.f/ far;
	}